
 // 设置异常等级， b[3:2]=el, b[1]=res0, b[0]=1:sp_elx, b[0]=0:sp_el0
// b[5:4] = 0x1 a32 stat
///////////////////////////////
// aarch32 exception level
// M[3:0] Mode
// 0b0000 User
// 0b0001 FIQ
// 0b0010 IRQ
// 0b0011 Supervisor
// 0b0110 Monitor
// 0b0111 Abort
// 0b1010 Hyp
// 0b1011 Undefined
// 0b1111 System
///////////////////////////////
// mov x0, #0x01d3  // a32-super SPSR_svc
// mov x0, #0x01d0 // a32-user
// mov x0, #0x01da // a32-hyp, 
// mov x0, #0x01d6 // a32-mon
// mov x0, #0x01df // a32-system
// mov x0, #0x03c5 //el1h
// mov x0, #0x03c9 // el2h
// mov x0, 0x03cd // el3h
// mov x0, 0x03cc // el3t
// orr x0, x0, #0x1<<4

#include "asm-a32.h"

.global _mon_vec
.global _hyp_vec
.global _sys_entry

.macro real_addr_calc out, lable
	adr r0, .
	ldr r1, =.
	add r0, r0, #4
	sub r0, r1, r0
	ldr r1, =\lable
	sub \out, r1, r0
.endm

.macro if_eq_b lable, arg1, arg2
	mov r1, \arg2
	cmp \arg1, r1
	beq \lable
.endm

_start:
	mrs r0, cpsr
	mov r1, #0xfff
	and r0, r0, r1
	if_eq_b _boot_svc_entry, r0, #0x1d3 // a32-svc, super
	if_eq_b _hyp_entry, r0, #0x1da // a32-hyp
	if_eq_b _mon_entry, r0, #0x1d6  // a32-mon
	if_eq_b _usr_entry, r0, #0x1d0  // a32-usr
	if_eq_b _und_entry, r0, #0x1db // a32-und
	b _falut_el_loop

_falut_el_loop:
	mov r0, #0x82000000
	mov sp, r0
	asm_print_u32 r0
	b _falut_el_loop

_boot_svc_entry:
	mov r0, #0x8f00000
	mov sp, r0

	// mon exception vector base addr, MVBAR
	real_addr_calc r0, _mon_vec
	MCR p15,0,r0,c12,c0,1
	// entry mon mode
	smc #0

_mon_entry:
	mov r0, #0x8e00000
	mov sp, r0

	// mon exception vector base addr, MVBAR
	real_addr_calc r0, _mon_vec
	MCR p15,0,r0,c12,c0,1

	// hyp exception vector base addr, HVBAR
	real_addr_calc r0, _hyp_vec
	MCR p15,4,r0,c12,c0,0

	// svc exception vector base addr, VBAR
	real_addr_calc r0, _svc_vec
	MCR p15,0,r0,c12,c0,0

	// set change to hyp state
	adr r0, _hyp_entry
	mov lr, r0
	mov r0, #0x01da
	msr spsr_mon, r0
	eret

_hyp_entry:
	mov r0, #0x8d00000
	mov sp, r0

	asm_print_u32 r0

	// hyp exception vector base addr, HVBAR
	real_addr_calc r0, _hyp_vec
	MCR p15,4,r0,c12,c0,0

	// svc exception vector base addr, VBAR
	real_addr_calc r0, _svc_vec
	MCR p15,0,r0,c12,c0,0

	adr r0, _sys_entry
	MSR ELR_hyp, r0

	mov r0, #0x01df // a32-sys
	msr spsr_hyp, r0
	eret

_sys_entry:
	mov r0, #0x8c00000
	mov sp, r0
_s:
	add r0, r0, #4	
	ldr r1, =_r0
	asm_puts r1
	asm_print_u32 r0
	b _sys_entry

	hvc #0

_und_entry:
	mov r0, #0x8b00000
	mov sp, r0

_usr_entry:
	mov r0, #0x8a00000
	mov sp, r0

.align 4
_r0:
	.string "sys_entry: r0: \0"

